using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Text;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Media3D;

using NewtonDynamics;

namespace WPFPhysics1
{
	public class VehicleBody : PolygonBody
	{
		private double tireDiameter;
		private Vector tireOffset;
		private CJointVehicle vehicle;
		private Collection<Tire> tires = new Collection<Tire>();

		public VehicleBody( CCollision collision, double width, double height, Vector[] vertices )
			: base( collision, vertices )
		{
			tireDiameter = 0.45;
			tireOffset = new Vector( -tireDiameter / 2, -tireDiameter / 2 );

			vehicle = new CJointVehicle( World );
			vehicle.CreateVehicle( new Vector3D( 0, 1, 0 ), this );

			vehicle.VehicleTireUpdate += new EventHandler<CVehicleTireUpdateEventArgs>( vehicle_VehicleTireUpdate );

			AddTire( vehicle, new Vector3D( -width * 0.365, height * 0.41, -0.1 ) );
			AddTire( vehicle, new Vector3D( width * 0.36, height * 0.42, -0.1 ) );
/*			AddTire( vehicle, new Vector3D( 0.5, 0.25, depth + 0.1 ) );
			AddTire( vehicle, new Vector3D( -0.5, 0.25, depth + 0.1 ) );
*/		}

		/// <summary>
		/// Create instance
		/// </summary>
		/// <param name="world">World</param>
		/// <param name="mass">Mass</param>
		/// <param name="width">Width</param>
		/// <param name="height">Height</param>
		/// <param name="depth">Depth</param>
		/// <returns>New instance</returns>
		public static VehicleBody Create( CWorld world, double mass, double width, double height, double depth )
		{
			double halfW = width / 2;
			double halfH = height / 2;

			Vector[] vertices = new Vector[]
			{
				new Vector( -halfW, halfH * 0.85 ),
				new Vector( halfW, halfH ),
				new Vector( halfW, halfH * 0.4 ),
				new Vector( halfW * 0.88, -halfH * 0.1 ),
				new Vector( halfW * 0.2, -halfH ),
				new Vector( -halfW * 0.75, -halfH ),
				new Vector( -halfW * 0.92, -halfH * 0.35 )
			};
			/*
			java.util.Vector vectors = new java.util.Vector();

			foreach ( Vector vector in vertices )
			{
				vectors.add( new java.awt.Point( 1000 + (int)( vector.X * 100 ), 1000 + (int)( vector.Y * 100 ) ) );
			}

			DePoly depoly = new DePoly();
			Algorithm alg = new Algorithm( depoly, vectors, false );

			alg.run();
			*/

			//mcdResult.

			CCollisionConvexPrimitives collision = new CCollisionConvexPrimitives( world );
			collision.CreateConvexHull( PolygonBody.CreateVertexCloud( vertices, depth ), Matrix3D.Identity );

			VehicleBody body = new VehicleBody( collision, width, height, vertices );

			InertialMatrix inertialMatrix = collision.CalculateInertialMatrix;

			body.MassMatrix = new MassMatrix( (float)mass, inertialMatrix.m_Inertia * mass );
			body.CentreOfMass = inertialMatrix.m_Origin;

			collision.Release();

			return body;
		}

		private void AddTire( CJointVehicle vehicle, Vector3D position )
		{
			// all real values
			float wheelWidth = 0.3f;
			float radius = (float)tireDiameter / 2;
			float wheelMass = 20.0f;
			float suspension_length = 0.35f;

			float tireSuspensionStiffness = wheelMass * 2f * 8; // 1.2f; // since the motor is in the front 

			// spring formula 
			float tireSuspensionSpring = ( tireSuspensionStiffness * 9.8f ) / suspension_length;

			// resonate speed 
			float resonate = (float)Math.Sqrt( tireSuspensionSpring );

			// little super critical y damped 
			float tireSuspensionShock = 2.0f * resonate;

			Matrix3D wheelMatrix = Matrix3D.Identity;
			wheelMatrix.Translate( position );

			CTire tire = vehicle.VehicleAddTire(
				wheelMatrix,
				new Vector3D( 0, 0, 1 ),
				wheelMass,
				wheelWidth,
				radius,
				tireSuspensionShock,
				tireSuspensionSpring,
				suspension_length,
				tireOffset,
				1 );

			tires.Add( new Tire( tireDiameter, tire ) );
		}

		void vehicle_VehicleTireUpdate( object sender, CVehicleTireUpdateEventArgs e )
		{
			foreach ( Tire tire in tires ) tire.Update();
		}

		/// <summary>
		/// Get tires
		/// </summary>
		public Collection<Tire> Tires { get { return tires; } }

		/// <summary>
		/// Dispose
		/// </summary>
		public override void Dispose()
		{
			tires.Clear();
			vehicle.Dispose();
			base.Dispose();
		}
	}
}